#include "Predictor.h"
#include "Logger.h"

Logger predicted_log("./../predicted_log.log");
CoordPredictor::CoordPredictor() //构造函数
{
    cv::FileStorage fs("./../Configure/Settings.xml", cv::FileStorage::READ);

    cv::Mat processNoise, measurementNoise;
    cv::Mat t;
    fs["kalman_Q"] >> processNoise;
    fs["kalman_R"] >> measurementNoise;
    fs["cam2gyro_offset"] >> cam2gyro_offset;
    fs["gun2cam_offset"] >> gun2cam_offset;
    fs.release();

    KF_Mutex.lock();

    KF = std::make_shared<cv::KalmanFilter>(state_num, measure_num, 0, CV_32F); //调用了opencv里面自带的卡尔曼滤波器

    cv::Mat H = (cv::Mat_<float>(measure_num, state_num) << 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0);

    // cv::Mat H = (cv::Mat_<float>(measure_num, state_num) << 1, 0, 0, 0, 0, 0,
    //                                                         0, 1, 0, 0, 0, 0,
    //                                                         0, 0, 1, 0, 0, 0);

    KF->processNoiseCov = processNoise;                        // Q 过程噪声
    KF->measurementNoiseCov = measurementNoise;                // R 测量噪声
    cv::setIdentity(KF->errorCovPost, cv::Scalar::all(1));     // P 卡尔曼增益
    cv::setIdentity(KF->transitionMatrix, cv::Scalar::all(1)); // F 状态转移矩阵
    KF->measurementMatrix = H;

    KF_Mutex.unlock();
}

cv::Mat CoordPredictor::getKFGain()
{
    std::lock_guard<std::mutex> lock(KF_Mutex);
    return KF->gain.clone();
}

cv::Point3f CoordPredictor::getWorldCoordExtern()
{
    std::lock_guard<std::mutex> lock(world_coord_extern_mutex);
    return world_coord_extern;
}

cv::Point3f CoordPredictor::getWorldCoordNextExtern()
{
    std::lock_guard<std::mutex> lock(world_coord_next_extern_mutex);
    return world_coord_next_extern;
}

void CoordPredictor::updateProcessNoise(const cv::Mat &newQ)
{
    std::lock_guard<std::mutex> lock(KF_Mutex);
    KF->processNoiseCov = newQ.clone();
}

void CoordPredictor::updateMeasurementNoise(const cv::Mat &newR)
{
    std::lock_guard<std::mutex> lock(KF_Mutex);
    KF->measurementNoiseCov = newR.clone();
}

void CoordPredictor::updateCam2GyroOffset(const cv::Point3f &offset)
{
    std::lock_guard<std::mutex> lock(KF_Mutex);
    cam2gyro_offset = offset;
}

void CoordPredictor::updateGun2CamOffset(const cv::Point3f &offset)
{
    std::lock_guard<std::mutex> lock(KF_Mutex);
    gun2cam_offset = offset;
}

void CoordPredictor::updateExtraPredictionTime(const int newint)
{
    std::lock_guard<std::mutex> lock(extra_prediction_time_Mutex);
    extra_prediction_time = newint;
}

cv::Point3f CoordPredictor::predict(cv::Point3f coord, SpinDetector &spin_detector, GyroPose gyro_pose)
{

    Eigen::Quaternionf q(gyro_pose.q_0, gyro_pose.q_1, gyro_pose.q_2, gyro_pose.q_3);

    cout << "SOLVE_PNP: " << coord << endl;
    world_coord = camera2world(q, coord, cam2gyro_offset);
    std::cout << "Quaternion: w=" << q.w() << ", x=" << q.x() << ", y=" << q.y() << ", z=" << q.z() << std::endl;
    std::cout << "world_coord_1:" << world_coord << std::endl;
    std::cout << "cam2gyro_offset:" << cam2gyro_offset << std::endl;
    return predict(world_coord, spin_detector, gyro_pose.timestamp);
}

cv::Point3f CoordPredictor::predict(cv::Point3f world_coord, SpinDetector &spin_detector, uint32_t timestamp)
{
    this->world_coord_extern_mutex.lock();
    this->world_coord_extern = world_coord;
    this->world_coord_extern_mutex.unlock();

    cv::Mat correct_state;
    cv::Mat single_predicted;
    float dt = (float)(timestamp - last_t) / 300.0;
    // dt=1600;
    last_t = timestamp;
    cout << "SOLVE_WORLD: " << world_coord << endl;
    cv::Mat measurement = (cv::Mat_<float>(measure_num, 1) << world_coord.x, world_coord.y, world_coord.z);

    KF_Mutex.lock();

    // cout << KF->processNoiseCov << endl;

    KF->transitionMatrix.at<float>(0, 1) = dt;
    KF->transitionMatrix.at<float>(2, 3) = dt;
    KF->transitionMatrix.at<float>(4, 5) = dt;

    single_predicted = KF->predict();
    std::cout << "Predicted state (single_predicted): \n" << single_predicted << std::endl;

    correct_state = KF->correct(measurement);
    correct_coord =
        cv::Point3f(correct_state.at<float>(0, 0), correct_state.at<float>(2, 0), correct_state.at<float>(4, 0));

    std::cout << "Kalman Gain K: \n" << KF->gain << std::endl;

    KF_Mutex.unlock();

    spin_detector.pushCoord(correct_coord);
    bool spin_flag = spin_detector.solverSpin(correct_coord);
    // spin_detector.wether_spin=spin_flag;
    setBS_coeff(correct_coord);

    pitch_time = compensate(correct_coord, dt);
    cout << "correct_state:" << correct_state << endl;
    cout << "Pitch: " << pitch_time.x << "Time: " << pitch_time.y << endl;
    cout << "BS: " << bullet_speed << endl;
    // if (spin_flag)
    // {
    //     cout << "SPIN_FLAG" << endl;
    //     return correct_coord;
    // }
    cv::Point3f world_next = predictNextpoint(correct_state, pitch_time.y + dt + extra_prediction_time + delay_time);

    this->world_coord_next_extern_mutex.lock();
    world_coord_next_extern = world_next;
    this->world_coord_next_extern_mutex.unlock();

    // predicted_log.log("world_next:" + std::to_string(correct_coord.x) + "," + std::to_string(correct_coord.y) + "," +
    // std::to_string(correct_coord.z) + "world_coord:" + std::to_string(world_coord.x) + "," +
    // std::to_string(world_coord.y) + "," + std::to_string(world_coord.z));
    //  predicted_log.log("KF_Gian:" + std::to_string(KF_gain.at<float>(1, 0)) + ",");
    return world_next;
}

void CoordPredictor::initState(cv::Point3f coord, GyroPose gyro_pose, bool spin_once_flag)
{
    Eigen::Quaternionf q(gyro_pose.q_0, gyro_pose.q_1, gyro_pose.q_2, gyro_pose.q_3);
    cv::Point3f world_coord = camera2world(q, coord, cam2gyro_offset);

    KF_Mutex.lock();

    if (spin_once_flag)
    {
        KF->statePost = (cv::Mat_<float>(state_num, 1) << world_coord.x, KF->statePost.at<float>(1, 0), world_coord.y,
                         KF->statePost.at<float>(3, 0), world_coord.z, KF->statePost.at<float>(5, 0));
    }
    else
    {
        KF->statePost = (cv::Mat_<float>(state_num, 1) << world_coord.x, 0, world_coord.y, 0, world_coord.z, 0);
        last_t = gyro_pose.timestamp;
    }

    KF_Mutex.unlock();
}

void CoordPredictor::setBulletSpeed(int bs)
{
    bullet_speed = (double)bs;
}
